
#ifndef _CAMERA_CAL_PINHOLE_CAMERA_HH_
#define _CAMERA_CAL_PINHOLE_CAMERA_HH_

#include "Ravl/Vector2d.hh"
#include "Ravl/Vector3d.hh"
#include "Ravl/Matrix3d.hh"
#include "Ravl/Stream.hh"
#include "Ravl/BinStream.hh"
#include "Ravl/SArray1d.hh"

namespace CameraCalN
{
	using namespace RavlN;

	class PinholeCameraC
	{
	public:
		PinholeCameraC()
		{
		}
		//: default constructor

		PinholeCameraC(const Matrix3dC& nK, const Matrix3dC& nR, const Vector3dC& nt) :
		_cx(nK[0][2]),
			_cy(nK[1][2]),
			_fx(nK[0][0]),
			_fy(nK[1][1]),
			_R(nR),
			_t(nt)
		{
		}

	public:
		RealT& cx() { return _cx; };
		RealT& cy() { return _cy; };
		RealT& fx() { return _fx; };
		RealT& fy() { return _fy; };
		Matrix3dC& R() { return _R; };
		Vector3dC& t() { return _t; };

		const RealT& cx() const { return _cx; };
		const RealT& cy() const { return _cy; };
		const RealT& fx() const { return _fx; };
		const RealT& fy() const { return _fy; };
		const Matrix3dC& R() const { return _R; };
		const Vector3dC& t() const { return _t; };

		void Project(Vector2dC& z, const Vector3dC& x) const
		{
			Vector3dC Rx = (_R * x) + _t;
			z[0] = _cx + _fx * Rx[0] / Rx[2];
			z[1] = _cy + _fy * Rx[1] / Rx[2];
		}

		bool ProjectCheck(Vector2dC& z, const Vector3dC& x) const
		{
			Vector3dC Rx = (_R * x) + _t;
			if (Rx[2] < 1E-3)
				return false;
			z[0] = _cx + _fx * Rx[0] / Rx[2];
			z[1] = _cy + _fy * Rx[1] / Rx[2];
			return true;
		}

		void ProjectJacobian(FMatrixC<2,3>& Jz, const Vector3dC& x) const
		{
			Vector3dC Rx = (_R * x) + _t;
			RealT r_Rx2_2 = 1.0 / (Rx[2]*Rx[2]);
			Jz[0][0] = _fx * (_R[0][0]*Rx[2] - _R[2][0]*Rx[0]) * r_Rx2_2;
			Jz[0][1] = _fx * (_R[0][1]*Rx[2] - _R[2][1]*Rx[0]) * r_Rx2_2;
			Jz[0][2] = _fx * (_R[0][2]*Rx[2] - _R[2][2]*Rx[0]) * r_Rx2_2;
			Jz[1][0] = _fy * (_R[1][0]*Rx[2] - _R[2][0]*Rx[1]) * r_Rx2_2;
			Jz[1][1] = _fy * (_R[1][1]*Rx[2] - _R[2][1]*Rx[1]) * r_Rx2_2;
			Jz[1][2] = _fy * (_R[1][2]*Rx[2] - _R[2][2]*Rx[1]) * r_Rx2_2;
		}

	protected:
		RealT _cx;
		RealT _cy;
		RealT _fx;
		RealT _fy;
		Matrix3dC _R;
		Vector3dC _t;
	};

	istream& operator>>(istream& s, PinholeCameraC& camera);
	ostream& operator<<(ostream& s, const PinholeCameraC& camera);
	BinIStreamC& operator>>(BinIStreamC& s, PinholeCameraC& camera);
	BinOStreamC& operator<<(BinOStreamC& s, const PinholeCameraC& camera);

	typedef SArray1dC<PinholeCameraC> PinholeArrayC;




};

#endif
